#pragma config(Hubs,  S1, HTMotor,  none,     none,     none)
#pragma config(Hubs,  S2, HTMotor,  HTMotor,  HTMotor,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     HTMC,           sensorI2CCustom)
#pragma config(Motor,  motorA,          A,             tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     A1,            tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     A2,            tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S2_C1_1,     A3,            tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S2_C1_2,     A4,            tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S2_C2_1,     A5,            tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_2,     A6,            tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C3_1,     A7,            tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C3_2,     A8,            tmotorTetrix, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//


#include "JoystickDriver.c"
//#include "drivers/common.h"
#include "drivers/hitechnic-compass.h"
//#include "drivers/HTAC-driver.h"
//#include "drivers/HTGYRO-driver.h"
//#include "drivers/HTIRS-driver.h"

void moveBackWheels( float distanceValue )
{
  nMotorEncoder[A1] = 0;

  while( nMotorEncoder[A1] < 100 * distanceValue )
  {
  motor[A1] = 100;
  motor[A2] = 100;
  motor[A3] = 30;
  motor[A4] = 30;
  }

  motor[A1] = 0;
  motor[A2] = 0;
  motor[A3] = 0;
  motor[A4] = 0;

}

void moveDiagonal1( float distanceValue )
{
  nMotorEncoder[A1] = 0;

  while( nMotorEncoder[A1] < 100 * distanceValue )
  {
  motor[A1] = 50;
  motor[A2] = 0;
  motor[A3] = 50;
  motor[A4] = 0;
  }

  motor[A1] = 0;
  motor[A3] = 0;

}

void moveDiagonal2( float distanceValue )
{
  nMotorEncoder[A2] = 0;

  while( nMotorEncoder[A2] < 100 * distanceValue )
  {
  motor[A1] = 0;
  motor[A2] = 100;
  motor[A3] = 0;
  motor[A4] = 100;
  }

  motor[A2] = 0;
  motor[A4] = 0;

}

float degreesRecenter( float degrees )
{
  while( degrees <= -180 || degrees > 180 )
  {
    if( degrees <= -180 )
    {
      degrees += 360;
    } else if( degrees > 180 ) {
      degrees -= 360;
    }
  }

  return degrees;
}


void absoluteTurn( int turnHeader ) //turnHeader between 0 - 360
{

  int currentAngle;
  do
  {

    currentAngle = HTMCreadRelativeHeading(HTMC);
    motor[A1] = 30 * sgn(degreesRecenter( turnHeader - currentAngle ));
    motor[A2] = -30 * sgn(degreesRecenter( turnHeader - currentAngle ));
    motor[A3] = -30 * sgn(degreesRecenter( turnHeader - currentAngle ));
    motor[A4] = 30 * sgn(degreesRecenter( turnHeader - currentAngle ));

  } while( currentAngle != turnHeader );

}



task main()
{
  int irsDirection;
  moveDiagonal1(10);

  /*
  HTMCsetTarget(HTMC);
  move(2.1); //recalibrate
  absoluteTurn(40);
  HTMCsetTarget(HTMC);
  wait10Msec(50);
  move(1.7);
  wait10Msec(100);
  move(1.9); */

  //IRS
  /*
  irsDirection = HTIRSreadDir(HTIRS);


  if(irsDirection < 5)
  {
    absoluteTurn(270);
    move(1); //recalibrate
    absoluteTurn(0);
    //lift up
    move(1); //recalibrate
  }

  else if(irsDirection > 5)
  {
    absoluteTurn(90);
    move(1); //recalibrate
    absoluteTurn(0);
    //lift up
    move(1); //recalibrate
  }
  else
  {
    //lift up
    move(5); //recalibrate
  }
  */

}
